#include "VisionController.h"

VisionController::VisionController(float horizontalTolerance,
		float verticalTolerance, int maxGhostFrames, int* epsilonFromCenter,
		int* epsilonStabilization) {
	this->camera = new VideoCapture();
	camera->open(CV_CAP_OPENNI);
	if (camera->grab()) {
		camera->retrieve(frame, CV_CAP_OPENNI_BGR_IMAGE);
	}
	this->state = ADAPTIVE_GRID;
	this->horizontalTolerance = horizontalTolerance;
	this->verticalTolerance = verticalTolerance;
	initThresholds();
	pthread_mutex_init(&mutex, 0);
	this->maxGhostFrames = maxGhostFrames;
	this->depthTrackingToleranceMin = 300;
	this->depthTrackingTolerance = DEPTHMAX - DEPTHMIN;
	this->lastMeanValue = 0;
	this->epsilonFromCenter = epsilonFromCenter;
	this->epsilonStabilization = epsilonStabilization;
	this->minTargetBottomY = frame.rows / 3;
	this->lastTargetIsValid = false;

	this->rightROIs = new vector<Rect*>();
	this->leftROIs = new vector<Rect*>();
	this->lastTarget = new Target();
//	int rightObstacleCenterX = (float) ((float) 3 / (float) 4) * frame.cols
//			+ epsilonFromCenter / 2;
//	int rightObstacleCenterY = frame.rows / 2;
	int rightObstacleCenterX = frame.cols / 2 + 100;
	int rightObstacleCenterY = frame.rows / 2;

	int rightObstacleTopLeftX = rightObstacleCenterX - obstacleROISide / 2;
	int rightObstacleTopLeftY = rightObstacleCenterY - obstacleROISide / 2;
	Rect* r1 = new Rect(rightObstacleTopLeftX, rightObstacleTopLeftY,
			obstacleROISide, obstacleROISide);
	Rect* r2 = new Rect(rightObstacleTopLeftX + obstacleROISide,
			rightObstacleTopLeftY, obstacleROISide, obstacleROISide);
	//inserisco dal più interno al più esterno
	rightROIs->push_back(r1);
	rightROIs->push_back(r2);

	//int leftObstacleCenterX = (frame.cols / 2 - epsilonFromCenter) / 2;
	int leftObstacleCenterX = frame.cols / 2 - 100;

	int leftObstacleCenterY = frame.rows / 2;
	int leftObstacleTopLeftX = leftObstacleCenterX - obstacleROISide / 2;
	int leftObstacleTopLeftY = leftObstacleCenterY - obstacleROISide / 2;
	Rect* l1 = new Rect(leftObstacleTopLeftX, leftObstacleTopLeftY,
			obstacleROISide, obstacleROISide);
	Rect* l2 = new Rect(leftObstacleTopLeftX - obstacleROISide,
			leftObstacleTopLeftY, obstacleROISide, obstacleROISide);
	//inserisco dal più interno al più esterno
	leftROIs->push_back(l2);
	leftROIs->push_back(l1);

	int frameCenterX = frame.cols / 2;
	int frameCenterY = frame.rows / 2;
	int topLeftX = frameCenterX - obstacleROISide / 2;
	int topLeftY = frameCenterY - obstacleROISide / 2;
	centerROI = new Rect(topLeftX, topLeftY, obstacleROISide, obstacleROISide);

}

VisionController::~VisionController() {
	camera->release();
	delete camera;
	rightROIs->clear();
	delete rightROIs;
	leftROIs->clear();
	delete leftROIs;
	delete lastTarget;
	thresholds.clear();

	pthread_mutex_destroy(&mutex);
}

void VisionController::preElaboration() {
	//FILTRAGGIO MEDIANO
	medianBlur(frame, frame, 7);
	//Filtraggio sulla profondità
	//inRange(depthMap, DEPTHMIN, DEPTHMAX, depthMap);

}

Rect VisionController::getNearestTarget(vector<Marker*>* markers) {
	uint16_t minDistance = DEPTHMAX;
	int minIndex = 0;

	for (int i = 0; i < markers->size(); i++) {
		uint16_t dist = getObjectDistance(markers->at(i)->getTotalROI(), false);
		//se il marker non è compreso in un range di tolleranza in profondità
		// lo scarto impostando la sua distanza a DEPTHMAX
//		cout << "lastTargetDistance: " << lastTarget->getDistance() << endl;
//		cout << "tolerance: " << depthTrackingTolerance << endl;
		if (abs(dist - lastTarget->getDistance()) > depthTrackingTolerance) {
			//cout << "Scartato" << endl;

			dist = DEPTHMAX;
		}

		if (minDistance > dist) {
			minDistance = dist;
			minIndex = i;
		}
	}

	// nel caso non sia sopravvissuto alcun marker valido restituisco il marker vuoto
	if (minDistance == DEPTHMAX)
		return Rect(0, 0, 0, 0);
	else
		return markers->at(minIndex)->getTotalROI();
}

Target* VisionController::selectFinalTarget(vector<Rect> roi) {

	Target* target = new Target();
	vector<bool> validPositions;
	for (int j = 0; j < roi.size(); j++)
		validPositions.push_back(true);
	map<int, int> indexAndCountMap;
	//identificazione dello stesso target in più roi
	for (int i = 0; i < roi.size(); i++) {

		if (validPositions.at(i)) {
			Rect a = roi.at(i);
			indexAndCountMap.insert(std::pair<int, int>(i, 1));
			for (int j = i + 1; j < roi.size(); j++) {
				if (validPositions.at(j)) {

					Rect b = roi.at(j);
					if (areSimilar(a, b, tenPercent * a.height)) {

						int count = indexAndCountMap.at(i) + 1;
						indexAndCountMap.at(i) = count;
						validPositions.at(j) = false;

					}
				}
			}

		}
	}
	int max = indexAndCountMap.at(0), imax = 0;
	map<int, int>::iterator it;

	for (it = indexAndCountMap.begin(); it != indexAndCountMap.end(); ++it) {
		if (it->second > max) {
			max = it->second;
			imax = it->first;
		}
	}
	Rect targetROI = roi.at(imax);
	rectangle(frame, targetROI, Scalar(0, 255, 0), 3);
	Mat hsvROI(HSV, targetROI);
	lastMeanValue = mean(hsvROI)[2];
	uint16_t depthDistance = getObjectDistance(targetROI, false);
	int centerX = targetROI.x + targetROI.width / 2;
	int distanceFromCenter = centerX - frame.cols / 2;
	target->setDistanceFromCenter(distanceFromCenter);
	target->setDistance(depthDistance);
	return target;
}

bool VisionController::areSimilar(Rect a, Rect b, int tolerance) {
	bool similar = false;
	int diffX = abs(a.x - b.x);
	int diffY = abs(a.y - b.y);
	int diffW = abs(a.width - b.width);
	int diffH = abs(a.height - b.height);
	if (diffX < tolerance && diffY < tolerance && diffW < tolerance
			&& diffH < tolerance)
		similar = true;
	return similar;
}

void VisionController::initThresholds() {
	vector<Threshold*>* thres1 = new vector<Threshold*>();
	vector<Threshold*>* thres2 = new vector<Threshold*>();
	//PRIMA SOGLIA
//	range primo
//	colore (top)
//	Threshold* thres1Color1 = new Threshold(Scalar(0, 0, 0), Scalar(15, 15, 15));
//	//range secondo colore
//	Threshold* thres1Color2= new Threshold(Scalar(200, 200, 200), Scalar(255, 255, 255));
//	//range terzo colore
////	Threshold thres1Color3(Scalar(157, 173, 168), Scalar(188, 255, 228));
//	thres1->push_back(thres1Color1);
//	thres1->push_back(thres1Color2);
//	thres1.push_back(thres1Color3);

	//SECONDA SOGLIA
//	Threshold* thres2Color1(Scalar(161, 216, 96), Scalar(191, 255, 141));
//	//range secondo colore
//	Threshold *thres2Color2(Scalar(27, 0, 52), Scalar(92, 192, 117));
//	//range terzo colore
//	//Threshold thres2Color3(Scalar(161, 216, 96), Scalar(191, 255, 141));
//	thres2->push_back(thres2Color1);
//	thres2->push_back(thres2Color2);
////	thres2.push_back(thres2Color3);
//
//	//TERZA SOGLIA
//	Threshold thres3Color1(Scalar(152, 217, 28), Scalar(255, 255, 82));
//	//range secondocolore
//	Threshold thres3Color2(Scalar(44, 79, 14), Scalar(84, 199, 64));
//	//range terzo colore
//	Threshold thres3Color3(Scalar(152, 217, 28),Scalar(255, 255, 82));
//	thres3.push_back(thres3Color1);
//	thres3.push_back(thres3Color2);
//	thres3.push_back(thres3Color3);
//
//
//	this->thresholds.push_back(thres1);
//	this->thresholds.push_back(thres2);
//	this->thresholds.push_back(thres3);
//	SOGLIA ADATTATIVA
	AdaptiveThreshold* threshColor1 = new AdaptiveThreshold(
			new RedThreshConfig());
	AdaptiveThreshold* threshColor2 = new AdaptiveThreshold(
			new GreenThreshConfig());
	thres1->push_back(threshColor1);
	thres1->push_back(threshColor2);
	//SOGLIA COSTANTE PER ELEVATA LUMINOSITA'
	AdaptiveThreshold* thres2Color1 = new AdaptiveThreshold(
			new BacklightRedThreshConfig());
	//	//range secondo colore
	AdaptiveThreshold* thres2Color2 = new AdaptiveThreshold(
			new BacklightGreenThreshConfig());
	thres2->push_back(thres2Color1);
	thres2->push_back(thres2Color2);

	this->thresholds.push_back(thres1);
	//this->thresholds.push_back(thres2);

}

// 0 se il bersaglio sta al centro
// 1 se il bersaglio è a sinistra del centro
// -1 se il bersaglio è a destra del centro
Target* VisionController::processFrame() {
	//Sezione critica
	Target* target = NULL;
	try {

		pthread_mutex_lock(&mutex);

		//Fotogramma attuale dalla camera
		if (camera->grab()) {
			camera->retrieve(frame, CV_CAP_OPENNI_BGR_IMAGE);
			camera->retrieve(depthMap, CV_CAP_OPENNI_DEPTH_MAP);

			preElaboration();

			cvtColor(frame, HSV, COLOR_BGR2HSV);
			//frame.copyTo(HSV);
			if (lastMeanValue == 0) {

				target = searchTarget(kernelRows, kernelCols);
			} else {

				target = searchTarget(HSV.rows, HSV.cols, lastMeanValue);
				//in caso di target non trovato con i valori di soglia tarati sull'ultimo marker trovato,
				//applico una analisi in base alla suddivisione dell'immagine in regioni
				if ((target->getDistance() == 0
						&& target->getDistanceFromCenter() == 0)
						|| target->getGhostFrames() > 0) {
					target = searchTarget(kernelRows, kernelCols);
					//state = ADAPTIVE_GRID;
				}
			}

			//restituisco un target trovato solo se lo trovo per almeno
			//due frame consecutivi
			if (!lastTargetIsValid) {
				this->lastTargetIsValid = target->isValid();
				target->setDistance((uint16_t) 0);
				target->setDistanceFromCenter(0);
				target->setGhostFrames(0);

			} else
				this->lastTargetIsValid = target->isValid();

			//aggiorno il last target e gestisco il ghost
			if (target->isValid()) {

				*lastTarget = *target;
			} else if (lastTarget->getGhostFrames() < maxGhostFrames) {
				//target non trovato, utilizzo il ghost per maxGhostFrames

				lastTarget->setGhostFrames(lastTarget->getGhostFrames() + 1);

				*target = *lastTarget;

			}
			// Marker ghost scaduto
			else {
				*lastTarget = *target;

			}

			//cout << target.getGhostFrames() << endl;
			int x = frame.cols / 2 + target->getDistanceFromCenter();
			line(frame, Point(x, 0), Point(x, frame.rows), Scalar(255, 0, 0),
					3);
			//VISUALIZZAZIONE
			drawLineEpsilon(*epsilonFromCenter);
			drawLineEpsilon(*epsilonStabilization);

			rectangle(frame, *centerROI, Scalar(255, 255, 255), 3);
			for (int i = 0; i < rightROIs->size(); i++) {
				rectangle(frame, *rightROIs->at(i), Scalar(255, 255, 255), 3);
				rectangle(frame, *leftROIs->at(i), Scalar(255, 255, 255), 3);
				stringstream leftDistanceString;
				leftDistanceString << getObjectDistance(*leftROIs->at(i), true)
						<< endl;
				stringstream rightDistanceString;
				rightDistanceString
						<< getObjectDistance(*rightROIs->at(i), true) << endl;
				putText(frame, leftDistanceString.str(),
						Point(leftROIs->at(i)->x, 50 + i * 50),
						FONT_HERSHEY_PLAIN, 1.5, Scalar(0, 255, 0), 2, 8);
				putText(frame, rightDistanceString.str(),
						Point(rightROIs->at(i)->x, 50 + i * 50),
						FONT_HERSHEY_PLAIN, 1.5, Scalar(0, 255, 0), 2, 8);
			}

			stringstream centerDistanceString;
			centerDistanceString << getFrontalDistance() << endl;

			putText(frame, centerDistanceString.str(), Point(centerROI->x, 50),
					FONT_HERSHEY_PLAIN, 1.5, Scalar(0, 255, 0), 2, 8);

			imshow("BGR", frame);
			waitKey(1);

		}
	} catch (...) {
		cout << "eccezione processFrame" << endl;

	}

	//cout << "lastTargetIsValid: " << lastTargetIsValid << endl;
	pthread_mutex_unlock(&mutex);
	if (target == NULL)
		target = new Target();
	return target;
}

Target* VisionController::searchTarget(int kernelRows, int kernelCols,
		double meanValue) {

	int direction = 0;
	vector<Rect> result;
	Target* target;
	//applico le n soglie, termina la prima che trova un target
	int j = 0;
	bool targetFound = false;
	while (j < thresholds.size() && !targetFound) {

		//init vettore matrici per filtraggio colore
		vector<Mat> colorFilteredMaps(thresholds.at(j)->size());
		//init vettore di componenti connesse.
		//per ogni matrice corrispondente al filtraggio su uno
		//specifico colore, conservo le componenti connesse che
		//superano una dimensione minima
		vector<vector<Rect> > connectedComponents(colorFilteredMaps.size());

		//processing per ogni matrice (colore)
		for (int i = 0; i < colorFilteredMaps.size(); i++) {
			//thresholding
			thresholds.at(j)->at(i)->getThresholdedImage(HSV,
					colorFilteredMaps.at(i), kernelRows, kernelCols, meanValue);
		}
		stringstream ss1;
		ss1 << "thresh1 " << j;
		stringstream ss2;
		ss2 << "thresh2 " << j;
		imshow(ss1.str(), colorFilteredMaps.at(0));
		imshow(ss2.str(), colorFilteredMaps.at(1));

		PatternFiltering patternFilter(patterFilteringConsecutiveBlacks);

		Mat resGreen, resRed;

		patternFilter.apply(colorFilteredMaps.at(0), colorFilteredMaps.at(1),
				colorFilteredMaps.at(0), colorFilteredMaps.at(1));

		//POST PROCESSING
		//				morphologyEx(colorFilteredMaps.at(i), colorFilteredMaps.at(i),
		//						MORPH_OPEN, Mat(), Point(-1, -1), 2);
		//				morphologyEx(colorFilteredMaps.at(i), colorFilteredMaps.at(i),
		//						MORPH_CLOSE, Mat(), Point(-1, -1), 2);
		for (int i = 0; i < colorFilteredMaps.size(); i++) {
			//connected components
			vector<vector<Point> > contours;
			Mat tempCopy;
			colorFilteredMaps.at(i).copyTo(tempCopy);
			findContours(tempCopy, contours, noArray(), CV_RETR_EXTERNAL,
					CV_CHAIN_APPROX_SIMPLE);
			for (int k = 0; k < contours.size(); k++) {
				Rect r = boundingRect(contours.at(k));
				//if (r.height >= 20 && r.width >= 20)
				connectedComponents.at(i).push_back(r);
				//rectangle(frame, r, Scalar(127 * i, 127 * i, 127 * i), 4);
			}

		}

		for (int i = 0; i < colorFilteredMaps.size(); i++) {
			stringstream ss;
			ss << "colore " << i + 1 << " soglia " << j + 1;
			imshow(ss.str(), colorFilteredMaps.at(i));
		}

		//UTILIZZO IL PRIMO COLORE ANCHE COME ULTIMO
		connectedComponents.push_back(connectedComponents.at(0));

		vector<Marker*>* markers = targetDetection(connectedComponents);

		Rect targetROI(0, 0, 0, 0);
		if (markers->size() > 0)
			targetROI = getNearestTarget(markers);

		for (int i = 0; i < markers->size(); i++) {
			rectangle(frame, markers->at(i)->getTotalROI(), Scalar(0, 0, 255),
					2);
		}
		if (targetROI.width > 0 && targetROI.height > 0) {
			result.push_back(targetROI);
			targetFound = true;
			//cv::rectangle(frame, targetROI, cv::Scalar(0, 0, 255));
			//			cout << "distanceFromCenter " << distanceFromCenter
			//					<< " depthDistance " << depthDistance << endl;
		}

		j++;
	}

	//selezione del marker finale
	if (result.size() > 0) {

		target = selectFinalTarget(result);

	} else
		target = new Target();
	depthTrackingTolerance = updateDepthTrackingTolerance();

	return target;
}

uint16_t VisionController::getObjectDistance(cv::Rect objectROI,
		bool useOnlyDepthMap) {
	vector<uint16_t> distanceValues;

	uint16_t distance = 0;

	//imshow("depth", depthMap);

	Mat region(this->depthMap, objectROI);
	for (int y = 0; y < region.rows; y++) {
		for (int x = 0; x < region.cols; x++) {
			distanceValues.push_back(region.at<uint16_t>(y, x));
		}
	}

	distance = getMedianValue(distanceValues);
	uint16_t visionDistance = calculateDistanceFromDimensions(objectROI);
	//se il valore fornito è nullo, cerchiamo di stimare la distanza
	//esclusivamente tramite la visione

	if (!useOnlyDepthMap && visionDistance > 0 && visionDistance < DEPTHMIN) {
		distance = visionDistance;

	}

	return distance;
}

uint16_t VisionController::getMedianValue(vector<uint16_t> values) {
	set<uint16_t> container;
	for (int i = 0; i < values.size(); i++) {
		container.insert(values.at(i));
	}
	set<uint16_t>::iterator it = container.begin();

	advance(it, container.size() / 2);
	//cout << (*it) << endl;

	return (*it);
}

vector<Marker*>* VisionController::targetDetection(
		vector<vector<Rect> > connectedComponents) {

	int colorsNumber = connectedComponents.size();
	Rect target(0, 0, 0, 0);
	vector<Marker*>* markers = new vector<Marker*>();
	for (int i = 0; i < connectedComponents.at(0).size(); i++) {

		Marker* m = new Marker(colorsNumber);
		m->setRegion(0, connectedComponents.at(0).at(i));
		markers->push_back(m);
		//cout << "post push_back" << endl;
	}

	for (int i = 0; i < colorsNumber - 1; i++) {
		//filtra le componenti tra due vettori (top, bottom)
		markers = filterComponents(markers, i, connectedComponents.at(i + 1));

	}
	//scarta componenti sottilissime in larghezza oppure
	//troppo troppo piccole in altezza oppure
	//troppo in cielo
	vector<Marker*>::iterator it = markers->begin();
	while (it != markers->end()) {
		Rect r = (*it)->getTotalROI();
		if (r.width < minTargetWidth || r.height < minTargetHeight
				|| (r.y + r.height) < minTargetBottomY) {
			it = markers->erase(it);
		} else
			++it;
	}

//	cout << "dist:" << getObjectDistance(target) << endl;
//	cout << target.height << " " << target.width << endl;
	return markers;
}

vector<Marker*>* VisionController::filterComponents(vector<Marker*>* markers,
		int index, vector<Rect> bottom) {
	vector<Marker*>* newMarkers = new vector<Marker*>();
	for (int i = 0; i < markers->size(); i++) {
		bool matched = false;
		int k = 0;
		while (!matched && k < bottom.size()) {
			if (areStacked(markers->at(i)->getRegion(index), bottom.at(k))) {

				markers->at(i)->setRegion(index + 1, bottom.at(k));
				newMarkers->push_back(markers->at(i));
				matched = true;
			}
			k++;
		}
	}
	markers->clear();
	delete markers;

	return newMarkers;
}

bool VisionController::areStacked(Rect top, Rect bottom) {
	int topCenterX = top.x + top.width / 2;
	int topCenterY = top.y + top.height / 2;
	int bottomCenterX = bottom.x + bottom.width / 2;
	int bottomCenterY = bottom.y + bottom.height / 2;
	int horizTresh = this->horizontalTolerance * (float) top.width;
	int vertTresh = this->verticalTolerance * (float) top.height;
	int diffY = bottomCenterY - topCenterY;
	int diffX = abs(topCenterX - bottomCenterX);
	if (diffY > 0 && diffY <= vertTresh && diffX <= horizTresh)
		return true;
	else
		return false;

}

uint16_t VisionController::calculateDistanceFromDimensions(cv::Rect objectROI) {

	//Distanza massima 70cm
	double width_mexp = 0.991951287, height_mexp = 0.994650302;
	double width_bexp = 146.639608, height_bexp = 134.0360399;
	int distanceEstimation = height_bexp * pow(height_mexp, objectROI.height);
	if (distanceEstimation > 0 && distanceEstimation < 70)
		return distanceEstimation * 10;		//distanza in mm
	else
		return 0;

}

uint16_t VisionController::getFrontalDistance() {

	return getObjectDistance(*centerROI, true);
}

uint16_t VisionController::updateDepthTrackingTolerance() {
	uint16_t tolerance;
	if (lastTarget->getDistance() == 0)
		tolerance = DEPTHMAX - DEPTHMIN;
	else
		tolerance = depthTrackingToleranceMin
				+ lastTarget->getGhostFrames()
						* (DEPTHMAX - DEPTHMIN - depthTrackingToleranceMin)
						/ maxGhostFrames;

	return tolerance;
}

void VisionController::drawLineEpsilon(int epsilon) {
	line(frame, Point(frame.cols / 2 - epsilon, 0),
			Point(frame.cols / 2 - epsilon, frame.rows - 1),
			Scalar(255, 255, 0), 3);
	line(frame, Point(frame.cols / 2 + epsilon, 0),
			Point(frame.cols / 2 + epsilon, frame.rows - 1),
			Scalar(255, 255, 0), 3);

}

vector<uint16_t> *VisionController::getRightObstaclesDistances() {
	return getObstaclesDistances(rightROIs);
}

vector<uint16_t> *VisionController::getLeftObstaclesDistances() {
	return getObstaclesDistances(leftROIs);
}

vector<uint16_t>* VisionController::getObstaclesDistances(vector<Rect*>* ROIs) {
	vector<uint16_t>* result = new vector<uint16_t>();
	for (int i = 0; i < ROIs->size(); i++) {
		uint16_t distance = getObjectDistance(*(ROIs->at(i)), true);
		if (distance > 0)
			result->push_back(distance);
		else
			result->push_back((uint16_t) DEPTHMAX);
	}
	return result;
}
